#!/usr/bin/env python3
# -*-coding:utf-8-*-
import rospy
import std_msgs.msg
from geometry_msgs.msg import Twist, PoseStamped
from sensor_msgs.msg import RegionOfInterest as ROI
import numpy as np

w = 640
h = 480



def local_pose_callback(data):
    global height,pid_w,pid_h
    height = data.pose.position.z   
    pid_w = [0.1*height, 0, 0] # pid parameters of yaw channel
    pid_h = [0.1*height, 0, 0] # pid parameters of up channel
    #rospy.loginfo(pid_w)

def callback(msg):
    global pid_w,pid_h
    tld_roi = msg
    cx = tld_roi.x_offset + tld_roi.width // 2
    cy = tld_roi.y_offset + tld_roi.height // 2
    area = tld_roi.width * tld_roi.width
    left_speed, forward_speed = pidtrack(cx, cy, area, w, h, pid_w, pid_h)
    rc = "forward: " + str(forward_speed) + "   left:" + str(left_speed) +"   p:" + str(pid_w[0])

    speed = Twist()
    if left_speed != 0  or forward_speed != 0:
        speed.linear.x = forward_speed
        speed.linear.y = left_speed
        speed.linear.z = 0.0
        speed.angular.x = 0.0
        speed.angular.y = 0.0
        speed.angular.z = 0.0
        rospy.loginfo(rc)
        pub.publish(speed)
    else:
        speed.linear.x = 0
        speed.linear.y = 0
        speed.linear.z = 0.0
        speed.angular.x = 0.0
        speed.angular.y = 0.0
        speed.angular.z = 0.0
        rospy.loginfo(rc)
        pub.publish(speed)

def pidtrack(cx, cy, area, w, h, pid_w, pid_h):
    ## PID
    # yaw channel
    error_w = cx - w // 2
    speed_w = pid_w[0] * error_w
    speed_w = int(np.clip(speed_w, -100, 100)) / 100.0

    # up channel
    error_h = cy- h // 2
    speed_h = pid_h[0] * error_h
    speed_h = int(np.clip(speed_h, -100, 100)) / 100.0

    # forward channel
    #error_f = np.sqrt(area) - 70
    #speed_f = pid_f[0] * error_f
    #speed_f = int(np.clip(speed_f, -100, 100)) / 100.0

    if cx != 0:
        left_speed = -speed_w
    else:
        left_speed = 0
    if cy != 0:
        forward_speed = -speed_h
    else:
        forward_speed = 0
    #if area != 0:
    #    forward_speed = 0
    #else:
    #    forward_speed = 0

    return left_speed, forward_speed
      


if __name__ == '__main__':

    rospy.init_node('pid', anonymous=True)
    sub = rospy.Subscriber("mavros/local_position/pose", PoseStamped, local_pose_callback,queue_size=1)
    sub1 = rospy.Subscriber("roi", ROI, callback)
    pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=1)
    rospy.spin()
